/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package FRC2856.Robot;

import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.KinectStick;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class ReboundRumbleBot extends IterativeRobot {
    
    //variables
    public Shooter cannon;// = new Firing();
    public DriveTrain driveT;// = new DriveTrain();
    public Aiming aimer;// = new Aiming(cannon);
    public Capture ballHandler;// = new Capture();
    public BridgeArm arm;//= new BridgeArm();
    public Autonomous auto;
    static int autoCount;
    public KinectStick kstickL;
    public KinectStick kstickR;
    public DashComm dash;
    
    public UserInterface UI;// = new UserInterface(cannon, driveT, aimer, ballHandler, arm);
    
    //public Autonomous auto = new Autonomous(aimer, cannon, driveT);
    
    //private SocketHandler sock;
    
    public void robotInit(){
        cannon = new Shooter();
        driveT = new DriveTrain();
        aimer = new Aiming(cannon, driveT);
        ballHandler = new Capture();
        arm = new BridgeArm();
        UI = new UserInterface(cannon, driveT, aimer, ballHandler, arm);
        auto = new Autonomous(aimer, cannon, driveT, arm, ballHandler);
        kstickL = new KinectStick(1);
        kstickR = new KinectStick(2);
        dash = new DashComm(driveT, cannon, ballHandler);
    }

    public void autonomousInit(){
        cannon.spinStop();
        auto.loopCount = 0;
        auto.autoDone = false;
        ballHandler.setUpperSpeed(0);
        ballHandler.setLowerSpeed(0);
    }
    
    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {
//        auto.run();
        //auto.pickABasket(0);//shoots at the top.
        driveT.tankDrive(kstickL, kstickR);
    }

    public void teleopInit(){
        cannon.spinStop();
    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic(){
        UI.update();
        dash.update();
    }
    
}
